#include "DrawCameraMarker.hpp"

#include <GL/gl.h>
#include <cmath>

DrawCameraMarker::DrawCameraMarker(double fov, double aspect, double z):
	fov(fov), aspect(aspect), z(z), colour(Eigen::Vector4f(1.0f, 1.0f, 1.0f, 1.0f)), emitTexCoords(false)
	{}
DrawCameraMarker::DrawCameraMarker(double fov, double aspect, double z, const Eigen::Vector4f& col):
	fov(fov), aspect(aspect), z(z), colour(col), emitTexCoords(false)
	{}

void DrawCameraMarker::render() const {
	const float FRONT_ALPHA = 1.0f;
	const float BACK_ALPHA = 0.3f;

	const double extentx = z * tan(fov / 2.0);
	const double extenty = extentx / aspect;

	glBegin(GL_LINES);
	// draw the image rectangle
	glColor4f(colour(0), colour(1), colour(2), BACK_ALPHA*colour(3));
	glVertex3d(-extentx, -extenty, z); glVertex3d(-extentx,  extenty, z);
	glVertex3d(-extentx,  extenty, z); glVertex3d( extentx,  extenty, z);
	glVertex3d( extentx,  extenty, z); glVertex3d( extentx, -extenty, z);
	glVertex3d( extentx, -extenty, z); glVertex3d(-extentx, -extenty, z);

	// draw x/y axis markers on the image rectangle
	glColor4f(0.0f, 1.0f, 0.0f, colour(3));
	glVertex3d(0.0, 0.0, z); glVertex3d(0.0, extenty, z);
	glColor4f(1.0f, 0.0f, 0.0f, colour(3));
	glVertex3d(0.0, 0.0, z); glVertex3d(extentx, 0.0, z);

	// join optical centre to corners of image rectangle
	glColor4d(colour(0), colour(1), colour(2), BACK_ALPHA*colour(3));
	glVertex3d(0.0, 0.0, 0.0); glVertex3d(-extentx, -extenty, z);
	glVertex3d(0.0, 0.0, 0.0); glVertex3d(-extentx,  extenty, z);
	glVertex3d(0.0, 0.0, 0.0); glVertex3d( extentx,  extenty, z);
	glVertex3d(0.0, 0.0, 0.0); glVertex3d( extentx, -extenty, z);
	glEnd();

	if (emitTexCoords) {
		glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
		glBegin(GL_QUADS);
		glTexCoord2d(0.0, 1.0); glVertex3d(-extentx, -extenty, z);
		glTexCoord2d(0.0, 0.0); glVertex3d(-extentx,  extenty, z);
		glTexCoord2d(1.0, 0.0); glVertex3d( extentx,  extenty, z);
		glTexCoord2d(1.0, 1.0); glVertex3d( extentx, -extenty, z);
		glEnd();
	}
}
